﻿using Kimd.Logic;
using Kimd.Motion;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.ExplorerBar;
using WorkStation.Common;
using Kimd.Machine;
using Kimd.Common;

namespace WorkStation.Logic
{
    /// <summary>
    /// 治具上料模组
    /// </summary>
    public class FeedTesterLogic : FlowBase
    {
        private FeedTesterStatus _status;
        private Axis _x;
        private Axis _y;
        private Axis _z;
        private Axis _r;
        private Axis _precisionY;
        private List<Cylinder> _grabCylinders;
        private List<Cylinder> _grabVacs;
        private List<Cylinder> _grabRevVacs;
        private List<Sensor> _grabVacSense;

        private List<Cylinder> _precisionCylinders;
        private List<Cylinder> _precisionVacs;
        private List<Cylinder> _precisionRevVacs;
        private List<Sensor> _precisionVacSense;

        private Tray _nozzleTray;
        private TesterStatusLogic[] _testerStatusLogics;
        public FeedTesterLogic(string flowName, FlowManager flowmanager) 
            : base(flowName, flowmanager)
        {
            _x = MachineResource.Instance.MachineAxis["测试移载X轴"];
            _y = MachineResource.Instance.MachineAxis["精定区Y轴"];
            _z = MachineResource.Instance.MachineAxis["测试移载Z轴"];
            _r = MachineResource.Instance.MachineAxis["测试移载U轴"];
            _precisionY = MachineResource.Instance.MachineAxis["精定区Y轴"];

            _grabCylinders = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["测试移载取料气缸1下降V"],
                MachineResource.Instance.MachineCylinder["测试移载取料气缸2下降V"],
                MachineResource.Instance.MachineCylinder["测试移载取料气缸3下降V"],
                MachineResource.Instance.MachineCylinder["测试移载取料气缸4下降V"],
            };
            _grabVacs = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["测试移载取料1真空吸"],
                MachineResource.Instance.MachineCylinder["测试移载取料2真空吸"],
                MachineResource.Instance.MachineCylinder["测试移载取料3真空吸"],
                MachineResource.Instance.MachineCylinder["测试移载取料4真空吸"],
            };
            _grabRevVacs = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["测试移载取料1破真空"],
                MachineResource.Instance.MachineCylinder["测试移载取料2破真空"],
                MachineResource.Instance.MachineCylinder["测试移载取料3破真空"],
                MachineResource.Instance.MachineCylinder["测试移载取料4破真空"],
            };
            _grabVacSense = new List<Sensor>()
            {
                MachineResource.Instance.MachineSensor["测试移载取料1真空达到"],
                MachineResource.Instance.MachineSensor["测试移载取料2真空达到"],
                MachineResource.Instance.MachineSensor["测试移载取料3真空达到"],
                MachineResource.Instance.MachineSensor["测试移载取料4真空达到"],
            };
            _precisionCylinders = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["精定位1定位气缸伸出V"],
                MachineResource.Instance.MachineCylinder["精定位2定位气缸伸出V"],
                MachineResource.Instance.MachineCylinder["精定位3定位气缸伸出V"],
                MachineResource.Instance.MachineCylinder["精定位4定位气缸伸出V"],
            };
            _precisionVacs = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["精定位1真空吸"],
                MachineResource.Instance.MachineCylinder["精定位2真空吸"],
                MachineResource.Instance.MachineCylinder["精定位3真空吸"],
                MachineResource.Instance.MachineCylinder["精定位4真空吸"],
            };
            _precisionRevVacs = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["精定位1破真空"],
                MachineResource.Instance.MachineCylinder["精定位2破真空"],
                MachineResource.Instance.MachineCylinder["精定位3破真空"],
                MachineResource.Instance.MachineCylinder["精定位4破真空"],
            };
            _precisionVacSense = new List<Sensor>()
            {
                MachineResource.Instance.MachineSensor["精定位1真空到位"],
                MachineResource.Instance.MachineSensor["精定位2真空到位"],
                MachineResource.Instance.MachineSensor["精定位3真空到位"],
                MachineResource.Instance.MachineSensor["精定位4真空到位"],
            };

            _nozzleTray = new Tray(1, 4);

            _testerStatusLogics = new TesterStatusLogic[2];
            _testerStatusLogics[0] = FlowManager.Instance.SelectFlow("测试机状态1") as TesterStatusLogic;
            _testerStatusLogics[1] = FlowManager.Instance.SelectFlow("测试机状态2") as TesterStatusLogic;

            RegisterDirectSignal($"测试治具1放料完成");
            RegisterDirectSignal($"测试治具2放料完成");
        }

        public override void Calibrate()
        {
            base.Calibrate();
        }

        public override void Home()
        {
            if (!CheckFlowCondition()) return;

            MultiCylinderOperate(_grabCylinders, 5000, new List<bool>() { false, false, false, false });
            if (!CheckFlowCondition()) return;
            //TODO 吸嘴真空判断
            _grabRevVacs.ForEach(v => v.Off());
            for (int i = 0; i < _grabVacs.Count; i++)
            {
                if (!_grabVacSense[i].WaitOn())
                {
                    _grabVacs[i].Off();
                }
            }

            WaitForSignal("取料逻辑", "x回零完成");
            if (!CheckFlowCondition()) return;
            _z.ClearAlarm();
            Thread.Sleep(200);
            _z.SetServoOn();
            Thread.Sleep(200);
            _x.ClearAlarm();
            Thread.Sleep(200);
            _x.SetServoOn();
            Thread.Sleep(200);
            _r.ClearAlarm();
            Thread.Sleep(200);
            _r.SetServoOn();
            Thread.Sleep(200);
            _precisionY.ClearAlarm();
            Thread.Sleep(200);
            _precisionY.SetServoOn();
            Thread.Sleep(200);

            if(!GlobalVariable.IsSystemAlreadyHome)
                SingleAxisGoHome(_z);
            if (!CheckFlowCondition()) return;
            if (!GlobalVariable.IsSystemAlreadyHome)
                AxisGoHomeSimultaneously(new List<Axis>() { _x, _precisionY });
            else
                AxisMoveSimultaneously(new List<Axis>() { _x, _precisionY }, new List<double>() { 0, 0 });
            if (!CheckFlowCondition()) return;
            if (!GlobalVariable.IsSystemAlreadyHome)
                SingleAxisGoHome(_r);
            else
                SingleAxisMove2Point(_r, 0);
            if (!CheckFlowCondition()) return;

            checkProductStatus();
            if (!CheckFlowCondition()) return;

            //AxisMoveSimultaneously(_x, _r, _z, "安全位置");
            //if (!CheckFlowCondition()) return;

            WaitForSignal("下料逻辑", "回零完成");

            //开始前，移动到安全位置
            Monitor.Enter(GlobalVariable.Tester1AreaLock);
            SingleAxisMove2Point(_r, "治具1放料位置");
            if (!CheckFlowCondition()) return;
            AxisMoveSimultaneously(_x, _r, "治具1放料位置");
            if (!CheckFlowCondition()) return;

            _status = FeedTesterStatus.Wait;
        }

        public override void Init(out string errInfo)
        {
            base.Init(out errInfo);
        }

        public override void InSingleRunMode()
        {
            base.InSingleRunMode();
        }
        public override void Method()
        {
            if (!CheckFlowCondition()) return;

            switch (_status)
            {
                case FeedTesterStatus.Wait:
                    //移动前准备
                    prepareMove();
                    if (!CheckFlowCondition()) return;

                    while (GlobalVariable.PrecisionTray.Helper.IsEmpty && !IsBreak())
                    {
                        Thread.Sleep(20);
                    }
                    _status = FeedTesterStatus.GetPrecision;

                    break;

                case FeedTesterStatus.GetPrecision:
                    //head2 进入精定位区，锁起来
                    Monitor.Enter(GlobalVariable.PrecisionAreaLock);
                    //Monitor.Exit(GlobalVariable.Tester1AreaLock);

                    List<bool> precisionStatus = GlobalVariable.PrecisionTray.Helper.Select(c => c.Status == CellStatus.HasProduct).ToList();
                    List<bool> robotStatus = _nozzleTray.Helper.Select(c => c.Status == CellStatus.NoProduct).ToList();
                    List<bool> cellStatus = new List<bool>();
                    for (int i = 0; i < precisionStatus.Count; i++)
                    {
                        cellStatus.Add(precisionStatus[i] && robotStatus[i]);
                    }
                    //确定跑起来以后，方可让出tester区域控制权
                    AxisMoveSimultaneously(_x, _y, "精定位取料位置", lockObj: GlobalVariable.Tester1AreaLock);
                    if (!CheckFlowCondition()) return;
                    SingleAxisMove2Point(_r, "精定位取料位置");
                    if (!CheckFlowCondition()) return;

                    NozzleGetProductExt(_z,
                                        _grabCylinders,
                                        _grabVacs,
                                        _grabVacSense,
                                        _precisionVacs,
                                        _precisionRevVacs,
                                        "精定位取料位置",
                                        "安全位置",
                                        "",
                                        5000,
                                        delayAfterOpenVac: 100,
                                        delayBeforeVacCheck: 100,
                                        GlobalVariable.IsDryRun,
                                        cellStatus);
                    if (!CheckFlowCondition()) return;
                    //交换产品状态
                    cellStatus.Select((c, index) => new { Cell = c, Index = index })
                              .Where(item => item.Cell)
                              .Select(item => item.Index).ToList().ForEach(index =>
                              {
                                  _nozzleTray.Helper.SetCellStatus(index, CellStatus.HasProduct);
                                  _nozzleTray.Helper.SetCellBarcode(index, GlobalVariable.PrecisionTray.Helper.GetCell(index).SN);
                                  _nozzleTray.Helper.SetCellResult(index, TestResult.NoTest);

                                  GlobalVariable.PrecisionTray.Helper.SetCellStatus(index, CellStatus.NoProduct);
                                  GlobalVariable.PrecisionTray.Helper.SetCellBarcode(index, string.Empty);
                                  GlobalVariable.PrecisionTray.Helper.SetCellResult(index, TestResult.NoTest);
                              });
                    if (!CheckFlowCondition()) return;

                    _status = FeedTesterStatus.FeedTester;

                    break;

                case FeedTesterStatus.FeedTester:
                    //看哪个位置可以去上料，从后往前搜索的原则
                    //如果没有位置可以放，优先去1号位等着，这样可以腾出地方上料
                    int idleTesterIndex = SelectIdleTester();
                    //如果找到了可以放料的Tester
                    if (idleTesterIndex != -1)
                    {
                        switch (idleTesterIndex)
                        {
                            case 0:
                                Monitor.Enter(GlobalVariable.Tester1AreaLock);
                                GoAndPutProduct(0);
                                if (!CheckFlowCondition()) return;
                                //占着tester1的坑位，等待精定位有料
                                while (GlobalVariable.PrecisionTray.Helper.IsEmpty && !IsBreak())
                                {
                                    Thread.Sleep(20);
                                }
                                if (!CheckFlowCondition()) return;
                                _status = FeedTesterStatus.GetPrecision;
                                break;
                            case 1:
                                //首先要拿到tester1的控制权，然后要拿到tester2的控制权
                                Monitor.Enter(GlobalVariable.Tester1AreaLock);
                                GoAndPutProduct(1);
                                if (!CheckFlowCondition()) return;
                                //占着tester1的坑位，等待精定位有料
                                while (GlobalVariable.PrecisionTray.Helper.IsEmpty && !IsBreak())
                                {
                                    Thread.Sleep(20);
                                }
                                if (!CheckFlowCondition()) return;
                                _status = FeedTesterStatus.GetPrecision;
                                break;
                        }
                    }

                    break;
            }
        }

        public override void OnStop()
        {
            base.OnStop();
        }

        public override bool PreStartCheck()
        {
            return base.PreStartCheck();
        }

        public override void SpotCheck()
        {
            base.SpotCheck();
        }
        public void prepareMove()
        {
            SingleAxisMove2Point(_z, "安全位置");
            if (!CheckFlowCondition()) return;
        }
        public int SelectIdleTester()
        {
            if (GlobalVariable.IsDryRun)
            {
                int idleTesterIndex = -1;
                List<TesterStatus> status = new List<TesterStatus>();
                status.Add((_flowManager.SelectFlow($"测试治具1") as SimTesterLogic).CurrentTesterStatus);
                status.Add((_flowManager.SelectFlow($"测试治具2") as SimTesterLogic).CurrentTesterStatus);
                List<bool> inUse = new List<bool>();
                inUse.Add(SystemConfig.Instance.UsingStatus.TesterInUseStatus[0]);
                inUse.Add(SystemConfig.Instance.UsingStatus.TesterInUseStatus[1]);

                //for (int i = status.Count - 1; i >= 0; i--)
                //{
                //    //只要有1个tester启用了，就认为整个模组启用
                //    if (inUse[i])
                //    {
                //        if (status[i] == TesterStatus.Idle)
                //        {
                //            idleTesterIndex = i;
                //            break;
                //        }
                //    }
                //}
                for (int i = 0; i < 2; i++)
                {
                    //只要有1个tester启用了，就认为整个模组启用
                    if (inUse[i])
                    {
                        if (status[i] == TesterStatus.Idle)
                        {
                            idleTesterIndex = i;
                            break;
                        }
                    }
                }
                return idleTesterIndex;
            }
            else
            {
                for (int i = 0; i < 2; i++)
                {
                    TesterStatusLogic testerLogic = (FlowManager.Instance.SelectFlow($"测试机状态{i + 1}") as TesterStatusLogic);
                    if (testerLogic.IsInAction(TesterActionInFlag.转盘允许取料))
                        return i;
                }
            }
            return -1;
        }
        public void GoAndPutProduct(int testerIndex)
        {
            //需要等到轴动起来以后，才能释放锁
            //Monitor.Exit(GlobalVariable.PrecisionAreaLock);
            SingleAxisMove2Point(_r, $"治具{testerIndex + 1}放料位置");
            if (!CheckFlowCondition()) return;
            AxisMoveSimultaneously(_x, _r, $"治具{testerIndex + 1}放料位置", lockObj: GlobalVariable.PrecisionAreaLock);
            if (!CheckFlowCondition()) return;
            NozzlePutProductExt(_z, _grabCylinders, _grabVacs, _grabRevVacs, null,
                                $"治具{testerIndex + 1}放料位置", "安全位置", 0, 0,
                                5000, delayBeforeNozzleUp: 100,
                                _nozzleTray.Helper.Select(c => c.Status == CellStatus.HasProduct).ToList());
            if (!CheckFlowCondition()) return;
            if (!GlobalVariable.IsDryRun)
            {
                //给出放料到位信号
                CommonMethod.SetOnoff(ref GlobalVariable.HandlerActionData[testerIndex],
                                      (int)HandlerActionOutFlag.上下料机放料到位,
                                      true);
                //等待转盘关闭允许放料信号
                while (!_testerStatusLogics[testerIndex].IsInAction(TesterActionInFlag.转盘允许放料) && !IsBreak())
                {
                    Thread.Sleep(20);
                }
                if (!CheckFlowCondition()) return;
                //关闭放料到位信号
                CommonMethod.SetOnoff(ref GlobalVariable.HandlerActionData[testerIndex],
                                      (int)HandlerActionOutFlag.上下料机放料到位,
                                      false);
            }
            else
            {
                Tray testerTray = (FlowManager.Instance.SelectFlow($"测试治具{testerIndex + 1}") as SimTesterLogic).TesterTray;

                for (int i = 0; i < 4; i++)
                {
                    if (_nozzleTray.Helper.GetCell(i).Status == CellStatus.HasProduct)
                    {
                        testerTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                        testerTray.Helper.SetCellBarcode(i, _nozzleTray.Helper.GetCell(i).SN);
                        testerTray.Helper.SetCellResult(i, TestResult.NoTest);

                        _nozzleTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                    }
                    else
                        testerTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                }
                SetSignal($"测试治具{testerIndex + 1}放料完成");
                if (!CheckFlowCondition()) return;
            }
        }
        public void checkProductStatus()
        {
        restart:
            for (int i = 0; i < _grabVacSense.Count; i++)
            {
                if (_grabVacSense[i].WaitOn())
                {
                    MachineMessageScheduler.Instance.Add(new MachineMessage(MessageType.InfoAndPause, $"测试头上有料，请取走", this.Description, 0, "系统"));
                    //出错，停下所有线程
                    _flowManager.Pause();
                    //若为报错，需重新检测启动信号
                    WaitForStart();
                    if (!CheckFlowCondition()) return;
                    goto restart;
                }
            }
            //关闭所有真空
            _grabVacs.ForEach(v => v.Off());
        }
    }
    public enum FeedTesterStatus
    {
        Wait,
        GetPrecision,
        FeedTester,
    }
}
